{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# Frobenius Norm Factors"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "This header defines factors that operate directly on the entries of rotation matrices (`Rot3` or generally `SO(n)`) rather than using their Lie algebra representation (log map). They minimize the Frobenius norm of the difference between rotation matrices.\n",
        "\n",
        "These factors can sometimes be useful in specific optimization contexts, particularly in rotation averaging problems or as alternatives to standard `BetweenFactor` or `PriorFactor` on rotations.\n",
        "\n",
        "*   `FrobeniusPrior<T>`: Penalizes the Frobenius norm difference between a variable rotation `T` and a fixed target matrix `M`. Error is $||T - M||_F^2$.\n",
        "*   `FrobeniusFactor<T>`: Penalizes the Frobenius norm difference between two variable rotations `T1` and `T2`. Error is $||T1 - T2||_F^2$.\n",
        "*   `FrobeniusBetweenFactor<T>`: Penalizes the Frobenius norm difference between the predicted rotation `T2` and the expected rotation `T1 * T12_measured`. Error is $||T1 \\cdot T_{12} - T2||_F^2$.\n",
        "**Note:** The noise models for these factors operate on the vectorized rotation matrix (e.g., 9 elements for `Rot3`). The helper function `ConvertNoiseModel` attempts to convert standard rotation noise models (like those for `BetweenFactor<Rot3>`) into an appropriate isotropic noise model for the Frobenius factor. It expects the input noise model to be isotropic."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/FrobeniusFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Rot3, Pose3, Values\n",
        "from gtsam import FrobeniusPriorRot3, FrobeniusFactorRot3, FrobeniusBetweenFactorRot3\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X\n",
        "R = symbol_shorthand.R # Using 'R' for Rot3 keys"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fprior_header_md"
      },
      "source": [
        "## 1. `FrobeniusPrior<Rot3>`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fprior_desc_md"
      },
      "source": [
        "Constrains a `Rot3` variable `R(0)` to be close to a target matrix `M` in the Frobenius norm sense."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "fprior_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "FrobeniusPriorRot3:   keys = { r0 }\n",
            "  noise model: unit (9) \n",
            "\n",
            "FrobeniusPrior error (vectorized matrix diff): 9.999916666944462e-05\n"
          ]
        }
      ],
      "source": [
        "target_matrix = Rot3.Yaw(0.1).matrix() # Target matrix (must be 3x3)\n",
        "key = R(0)\n",
        "\n",
        "# Create a standard isotropic noise model for rotation (3 dimensional)\n",
        "rot_noise_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.01)\n",
        "\n",
        "# Convert it for the Frobenius factor (9 dimensional)\n",
        "frobenius_noise_model_prior = gtsam.noiseModel.Isotropic.Sigma(9, 0.01) # Or use ConvertNoiseModel\n",
        "\n",
        "prior_fro = FrobeniusPriorRot3(key, target_matrix, frobenius_noise_model_prior)\n",
        "prior_fro.print(\"FrobeniusPriorRot3: \")\n",
        "\n",
        "# Evaluate error\n",
        "values = Values()\n",
        "values.insert(key, Rot3.Yaw(0.11)) # Slightly different rotation\n",
        "error_prior = prior_fro.error(values)\n",
        "print(f\"\\nFrobeniusPrior error (vectorized matrix diff): {error_prior}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ffactor_header_md"
      },
      "source": [
        "## 1. `FrobeniusFactor<Rot3>`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ffactor_desc_md"
      },
      "source": [
        "Constrains two `Rot3` variables `R(0)` and `R(1)` to be close to each other in the Frobenius norm sense."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {
        "id": "ffactor_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "FrobeniusFactorRot3:   keys = { r0 r1 }\n",
            "  noise model: unit (9) \n",
            "\n",
            "FrobeniusFactor error (vectorized matrix diff): 2.499994791671017e-05\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "\n",
        "key1 = R(0)\n",
        "key2 = R(1)\n",
        "# Use same noise model dimension (9)\n",
        "frobenius_noise_model_between = gtsam.noiseModel.Isotropic.Sigma(9, 0.02)\n",
        "\n",
        "factor_fro = FrobeniusFactorRot3(key1, key2, frobenius_noise_model_between)\n",
        "factor_fro.print(\"\\nFrobeniusFactorRot3: \")\n",
        "\n",
        "# Evaluate error\n",
        "values.insert(key1, Rot3.Yaw(0.11))\n",
        "values.insert(key2, Rot3.Yaw(0.115)) # R1 slightly different from R0\n",
        "error_factor = factor_fro.error(values)\n",
        "print(f\"\\nFrobeniusFactor error (vectorized matrix diff): {error_factor}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fbetween_header_md"
      },
      "source": [
        "## 3. `FrobeniusBetweenFactor<Rot3>`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fbetween_desc_md"
      },
      "source": [
        "Acts like `BetweenFactor<Rot3>` but minimizes $||R_1 \\cdot R_{12} - R_2||_F^2$ instead of using the Log map error."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {
        "id": "fbetween_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor<class gtsam::Rot3>(r0,r1)\n",
            "  T12:  [\n",
            "\t0.999988, -0.00499998, 0;\n",
            "\t0.00499998, 0.999988, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "  noise model: unit (9) \n",
            "\n",
            "FrobeniusBetweenFactor error: 1.925929944387236e-34\n"
          ]
        }
      ],
      "source": [
        "measured_R12 = Rot3.Yaw(0.005)\n",
        "# Use same noise model dimension (9)\n",
        "frobenius_noise_model_b = gtsam.noiseModel.Isotropic.Sigma(9, 0.005)\n",
        "\n",
        "between_fro = FrobeniusBetweenFactorRot3(key1, key2, measured_R12, frobenius_noise_model_b)\n",
        "between_fro.print(\"\\nFrobeniusBetweenFactorRot3: \")\n",
        "\n",
        "# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))\n",
        "error_between = between_fro.error(values)\n",
        "print(f\"\\nFrobeniusBetweenFactor error: {error_between}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
